#include <Wire.h>
#include <Arduino.h>
#include <string>

#include "mpu.h"
#include "lattice.h"

const int MPU_addr=0x68; // I2C address of the MPU-6050
short AcX,AcY,AcZ,GyX,GyY,GyZ;

/**
 * 初始化Mpu模块
 */
void mpuInit(int sda = 0, int scl = 0) {
    Wire.begin(sda, scl);
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x6B); // PWR_MGMT_1 register
    Wire.write(0); // set to zero (wakes up the MPU-6050)
    Wire.endTransmission(true);
}

//TODO 代完善 -车辆静止状态监测
unsigned long lastTime; //上次检测时间
int status = -1; //当前状态 -1=未初始化
unsigned long activityTime = millis(); //上次活动的时间，上次变换动画的时间
int activityType = 0; //上次活动类型

void mpuLoop() {

    unsigned long time = millis();
    if(time - 500 < lastTime){
        return;
    }
    lastTime = time;

    if(time > 10000 && time - activityTime > 10000 && status != 0){
        //长期保持一个动画，重置数据
        status = -1;
        activityTime = time;
        Serial.println("=== 重置陀螺仪 ===");
        return;
    }

    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14,true); // request a total of 14 registers
    short AcX2 = Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
    short AcY2 = Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    short AcZ2 = Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
//    short Tmp2 = Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
    short GyX2 = Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    short GyY2 = Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    short GyZ2 = Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

    //动态数据 -距离上次检测的差值
    short dAcX = AcX2 - AcX;
    short dAcY = AcY2 - AcY;
    short dAcZ = AcZ2 - AcZ;
    short dGyX = GyX2 - GyX;
    short dGyY = GyY2 - GyY;
    short dGyZ = GyZ2 - GyZ;

    //赋值
//    AcX = AcX2;
//    AcY = AcY2;
//    AcZ = AcZ2;
//    GyX = GyX2;
//    GyY = GyY2;
//    GyZ = GyZ2;

//    if(abs(dAcX) > 500){
//        if(dAcX > 0){
//            Serial.println("加速++++");
//            setAnimation(1);
//
//        }else{
//            Serial.println("减速----");
//            setAnimation(0);
//        }
//    }

    if(status == -1){
        //初始化
        status = 0;
        //赋值
        AcX = AcX2;
        AcY = AcY2;
        AcZ = AcZ2;
        GyX = GyX2;
        GyY = GyY2;
        GyZ = GyZ2;
    }else if(abs(dAcZ) > 2500){
        if(dAcZ > 0){
            if(status == 2){
                status = 0;
            }else{
                status = 1;
                Serial.println("加速+++");
                setAnimation(4, 0);
                if(activityType != 4){
                    activityType = 4; //活动类型 =动画ID
                    activityTime = millis(); //记录活动时间
                }
            }
        }else{
            if(status == 1){
                status = 0;
            }else{
                status = 2;
                Serial.println("减速---");
                setAnimation(5, 0);
                if(activityType != 5){
                    activityType = 5; //活动类型 =动画ID
                    activityTime = millis(); //记录活动时间
                }
            }
        }
    }else if(abs(dAcY) > 1200){
        if(dAcY > 0){
            if(status == 2){
                status = 0;
            }else{
                status = 1;
                Serial.println("右转>>>");
                setAnimation(3, 0);
                if(activityType != 3){
                    activityType = 3; //活动类型 =动画ID
                    activityTime = millis(); //记录活动时间
                }
            }
        }else{
            if(status == 1){
                status = 0;
            }else{
                status = 2;
                Serial.println("左转<<<");
                setAnimation(2, 0);
                if(activityType != 2){
                    activityType = 2; //活动类型 =动画ID
                    activityTime = millis(); //记录活动时间
                }
            }
        }
    }

    //TODO 转弯之后，自动恢复为前进方向

//    Serial.print("Ac = "); Serial.print(abs(dAcX)< 100?0:dAcX);
//    Serial.print(", "); Serial.print(abs(dAcY)< 500?0:dAcY);
//    Serial.print(","); Serial.println(abs(dAcZ)< 500?0:dAcZ);


//    Serial.print(" | Gy = "); Serial.print(dGyX< 500?0:dGyX);
//    Serial.print(", "); Serial.print(dGyY< 500?0:dGyY);
//    Serial.print(", "); Serial.println(dGyZ< 500?0:dGyZ);

//    Serial.print("Ac = "); Serial.print(dAcX); // 旋转角度 +加速 -减速
//    Serial.print(", "); Serial.print(dAcY); //旋转角度 +左转 -右转
//    Serial.print(","); Serial.println(dAcZ); // 旋转角度 +加速 -减速
//    Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet
//    Serial.print(" | Gy = "); Serial.print(GyX);
//    Serial.print(", "); Serial.print(GyY);
//    Serial.print(", "); Serial.println(GyZ);

    //判断是否静止状态


}